#include "mylib.h"

namespace mylib
{

  template <class T>
  struct eigen
  {
    int n;
    matrix<T> A;
    matrix<T> HessenbergA;
    eigen(int nn, const T *AA) : n(nn), A(nn, nn, AA), HessenbergA(nn, nn, AA){};
    ~eigen(){};
    void Householder();
    void Givens();
    void QR(int niter);
    void shift(T mu);

  }; // class eigen

  template <class T>
  void eigen<T>::Householder()
  {

    for (int i = 0; i < n - 2; i++)
    {
      vector<T> w = HessenbergA.selcol(i, i + 1, n);
      vector<T> x(w.size(), 0.0);
      x[0] = w.normal();
      HessenbergA.setcol(i, i + 1, x);

      w[0] = w[0] + w.normal();
      T w2 = pow(w.normal(), 2);
      T wdotx;
      vector<T> wx(w.size());

      for (int coli = i + 1; coli < n; coli++)
      {
        x = HessenbergA.selcol(coli, i + 1, n);
        wdotx = 2 * w.dot(x) / w2;
        wx = w * wdotx;
        x = x - wx;
        HessenbergA.setcol(coli, i + 1, x);
      }

      for (int rowi = 0; rowi < n; rowi++)
      {
        x = HessenbergA.selrow(rowi, i + 1, n);
        wdotx = 2 * w.dot(x) / w2;
        wx = w * wdotx;
        x = x - wx;
        HessenbergA.setrow(rowi, i + 1, x);
      }
    }
  } // Householder

  template <class T>
  void eigen<T>::Givens()
  {
    T theta;
    vector<T> c(n), s(n);

    for (int k = 0; k < n - 1; k++)
    {
      vector<T> x1(n - k), x2(n - k);
      if (abs(HessenbergA[k][k]) < 1e-15)
      {
        s[k] = 1;
        c[k] = 0;
      }
      else
      {
        theta = atan(-HessenbergA[k + 1][k] / HessenbergA[k][k]);
        c[k] = cos(theta);
        s[k] = sin(theta);
      }
      x1 = HessenbergA.selrow(k, k, n);
      x2 = HessenbergA.selrow(k + 1, k, n);
      HessenbergA.setrow(k, k, x1 * c[k] - x2 * s[k]);
      HessenbergA.setrow(k + 1, k, x1 * s[k] + x2 * c[k]);
      HessenbergA[k + 1][k] = T(0.0);
    }

    for (int k = 0; k < n - 1; k++)
    {
      vector<T> x1(k + 2), x2(k + 2);
      x1 = HessenbergA.selcol(k, k, x1.size());
      x2 = HessenbergA.selcol(k + 1, k, x2.size());
      HessenbergA.setcol(k, k, x1 * c[k] - x2 * s[k]);
      HessenbergA.setcol(k + 1, k, x1 * s[k] + x2 * c[k]);
    }

    for (int k = 0; k < n - 1; k++)
    {
      if (abs(HessenbergA[k + 1][k]) < 1e-15)
      {
        HessenbergA[k + 1][k] = 0.0;
      }
    }
  } // Givens

  template <class T>
  void eigen<T>::shift(T mu)
  {
    for (int i = 0; i < n; i++)
    {
      HessenbergA[i][i]+=mu;
    }
  }

  template <class T>
  void eigen<T>::QR(int niter)
  {
    Householder();
    for (int i = 0; i < niter; i++)
    {
      Givens();
    }
    HessenbergA.show(n, n);
  }
}